# 确保已安装 scipy 库
# 可以通过命令行安装: pip install scipy

from scipy.io import loadmat
import pandas as pd


#################################################### 读取.mat原始数据 #################################################
def read_mat_measure(path_mat_measure):
    # 加载 .mat 文件。 loadmat 返回一个字典，键是变量名，值是对应的 numpy 数组
    # mat_data = loadmat('Measure.mat')
    mat_data = loadmat(path_mat_measure)
    # 从加载的数据中获取 'TimeRPhi' 变量。 注意：变量名需要与 .mat 文件中实际保存的变量名完全一致
    TimeRPhi = mat_data['TimeRPhi']
    # 结果赋值给 Z。提取第 2 到第 3 行（对应 Python 索引 1:3）和所有列 (:)
    Z = TimeRPhi[1:3, :]
    # 打印 Z 的形状和内容以验证
    # print("Shape of Z:", Z.shape)
    # print("Content of Z:\n", Z)
    return Z


def read_mat_real(path_mat_real):
    # 加载 .mat 文件。 loadmat 返回一个字典，键是变量名，值是对应的 numpy 数组
    mat_data = loadmat(path_mat_real)
    # 从加载的数据中获取 'TXYdXdYAxAyTPhi' 变量。 注意：变量名需要与 .mat 文件中实际保存的变量名完全一致
    TXYdXdYAxAyTPhi = mat_data['TXYdXdYAxAyTPhi']
    # 结果赋值给 Z
    Xr = TXYdXdYAxAyTPhi
    # 打印 Z 的形状和内容以验证
    print("Shape of Xr:", Xr.shape)
    print("Content of Xr:\n", Xr)
    return Xr


####################################################### .mat原始数据坐标转换 #######################################################
def format_matlab_caekf(path_csv_dataset, path_csv_xy, path_csv_coord):
    # 1. 读取 dataset 的经纬度数据
    df = pd.read_csv(path_csv_dataset)
    latitudes_ori, longitudes_ori = df['lati_smooth'].values, df['long_smooth'].values
    latitudes_ori, longitudes_ori = latitudes_ori[1:], longitudes_ori[1:]
    lati_0, long_0 = latitudes_ori[0], longitudes_ori[0]
    # 2. 读取 CAEKF 算法结果 .csv 并获取 x 和 y
    df_transposed = read_csv_transpose(path_csv_xy)
    caekf_x = df_transposed.iloc[:, 0]    # 第一列: x
    caekf_y = df_transposed.iloc[:, 1]    # 第二列: y
    print_col_first_max_min_mean(caekf_x, "caekf_x")
    print_col_first_max_min_mean(caekf_y, "caekf_y")
    # 3. 将 x 和 y 转化为经纬度
    latitudes, longitudes = xy_to_latilong(lati_0, long_0, caekf_x, caekf_y)
    print_col_first_max_min_mean(longitudes, "longitudes")
    print_col_first_max_min_mean(latitudes, "latitudes")
    # 打印结果示例
    print("转换后的经纬度坐标:")
    for i in range(min(5, len(longitudes))):  # 只打印前5个作为示例
        print(f"Point {i}: Longitude = {longitudes[i]:.8f}, Latitude = {latitudes[i]:.8f}")
    print("----------------------------------------------------------------")
    # 4. 保存到 CSV 文件
    result_df = pd.DataFrame({
        'Longitude_ori': longitudes_ori,
        'Latitude_ori': latitudes_ori,
        'Longitude': longitudes,
        'Latitude': latitudes,
        'caekf_x': caekf_x,
        'caekf_y': caekf_y
    })
    result_df.to_csv(path_csv_coord, index=False)
    print(f"\n结果已保存到 {path_csv_coord}")


########################################################## 主函数 ###################################################
def main():
    # read_mat_measure('Measure.mat')
    read_mat_real('Real.mat')


if __name__=="__main__":
    main()